#ifndef MZ_ROBOT_NAVIGATION
#define MZ_ROBOT_NAVIGATION
#include <ros/ros.h>
#include "../double_buffer/double_buffer.h"
#include "../fliter/kalman_fliter.h"
#include <geometry_msgs/Twist.h>
#include <tf2_msgs/TFMessage.h>
#include <cmath>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <atomic>
#include <mutex>
#include <cstring>
#include <iostream>

struct NavPoint
{
    float x;
    float y;
};

class Navigation
{
public:
    static Navigation* Get();

    void init(ros::Publisher _publisher);
    void UpdatePosition(const tf2_msgs::TFMessage::ConstPtr& msg);


    void MoveForward(float length);
    void MoveBackward(float length);
    void TurnRight(float angle);
    void TurnLeft(float angle);

    void BackRight(float angle);
    void BackLeft(float angle);

protected:
    Navigation();

private:
    static Navigation* m_handel;

    KalmanFilter x_kf;
    KalmanFilter y_kf;
    KalmanFilter t_kf;

    ros::Publisher publisher;
    DoubleBuffer<NavPoint> nowPosion;
    DoubleBuffer<float> towards;

    float odd_x;
    float odd_y;
    float odd_w;
};

void delayMilliseconds(int milliseconds);
float GetToward(float x, float y, float z, float w);

#endif